#ifndef BSPLINE_SMOOTH_H
#define BSPLINE_SMOOTH_H

#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace Planner
{
    using Eigen::Vector3d;
    using std::vector;

    class BsplineSmooth
    {
    private:
        int k_;   // B样条阶数
        double gap_; // 路径间隙

        // deBoor算法迭代生成B样条曲线
        double deBoor(const vector<Vector3d> &traj, const vector<double> &T, int k, int r, double t, int j, int length, int dimenson);

    public:
        BsplineSmooth(){};
        ~BsplineSmooth(){};

        void setParams(int k, double gap)
        {
            k_ = k;
            gap_ = gap;
        };

        void smoothPath(const vector<Vector3d> &pts, vector<Vector3d> &smooth_pts);

        void samplePath(const vector<Vector3d> &pts, vector<Vector3d> &sample_pts);
    };

    double BsplineSmooth::deBoor(const vector<Vector3d> &traj, const vector<double> &T, int k, int r, double t, int j, int length, int dimenson)
    {
        double x;
        if (r == 0)
            if (j - 2 >= 0)
                if (j - 2 <= length - 1)
                    return traj[j - 2][dimenson];
                else
                    return traj[length - 1][dimenson];
            else
                return traj[0][dimenson];
        else if (T[j + k - r] - T[j] == 0)
            return 0;
        else
            x = (t - T[j]) / (T[j + k - r] - T[j]) * deBoor(traj, T, k, r - 1, t, j, length, dimenson) + ((T[j + k - r] - t) / (T[j + k - r] - T[j])) * deBoor(traj, T, k, r - 1, t, j - 1, length, dimenson);
        return x;
    }

    void BsplineSmooth::smoothPath(const vector<Vector3d> &pts, vector<Vector3d> &smooth_pts)
    {
        smooth_pts.clear();
        int i, j, n, length = pts.size();
        double x, y, z, t;
        vector<double> T;
        for (i = 0; i < length + k_ + 3; i++)
            T.push_back(i);
        if (length > k_)
        {
            for (j = k_ - 1; j < length + 3; j++)
            {
                for (t = T[j]; t < T[j + 1]; t = t + 0.1)
                {
                    x = deBoor(pts, T, k_, k_ - 1, t, j, length, 0);
                    y = deBoor(pts, T, k_, k_ - 1, t, j, length, 1);
                    z = deBoor(pts, T, k_, k_ - 1, t, j, length, 2);
                    smooth_pts.push_back(Eigen::Vector3d(x, y, z));
                }
            }
        }
    }

    void BsplineSmooth::samplePath(const vector<Vector3d> &pts, vector<Vector3d> &sample_pts)
    {
        sample_pts.clear();
        sample_pts.push_back(pts.front());
        for (int i = 1; i < pts.size() - 1; i++)
        {
            double dis = (pts[i] - sample_pts.back()).norm();
            if (dis > gap_)
                sample_pts.push_back(pts[i]);
        }
        double dis = (pts.back() - sample_pts.back()).norm();
        if (dis < 0.5 * gap_ && sample_pts.size() > 1)
            sample_pts.pop_back();
        sample_pts.push_back(pts.back());
    }

} // namespace Planner

#endif